/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.create.CreateDrive;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.CreateTeleopController;
import com.grt192.controller.spot.SensorServer;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.CreateBase;
import com.grt192.mechanism.SPOTDriverStation;
import com.grt192.mechanism.spot.SLight;
import com.grt192.prototyper.PrototyperUtils;
import com.grt192.sensor.spot.NetSensor;
import com.grt192.sensor.spot.NetServantSensor;
import com.grt192.utils.Util;
import com.iaroc.irobot.IRobotCreate;

/**
 * TeleopDriver drives arcade style by reading a DemoJoystick from a Telemetry
 * Server.
 * 
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class TeleopDriverMacro extends GRTObject implements Macro,
		TelemetryPorts, SensorChangeListener {

	private SLight[] lights = SLight.get();
	private int createLight = 2;
	private int joystickLight = 7;
	// base
	IRobotCreate irc;
	CreateDrive dt;
	CreateBase base;
	// ds
	NetSensor joystick;
	NetServantSensor joy;
	SensorServer encoders;
	SPOTDriverStation ds;
	// controller
	CreateTeleopController op;

	public TeleopDriverMacro() {
		setPrinting(true);
	}

	public int execute() {
		log("Initializing Base...");
		lights[createLight].rawColor(GRTDemoLED.Color.ORANGE);
		irc = new IRobotCreate();
		dt = new CreateDrive(irc);
		dt.start();
		base = new CreateBase(irc, dt);
		lights[createLight].rawColor(GRTDemoLED.Color.GREEN);
		log("Base init success!");

		log("Initializing Sensor service...");
		// encoders = new TelemetryServer(base.getVelocityEncoder(),
		// VELOCITY_ENCODER_PORT);
		// encoders.setPrinting(true);
		// encoders.start();
		log("Sensor server init success!");

		log("Initializing Driver...");
		joy = new NetServantSensor(PrototyperUtils.getAddress(1,
				lights[joystickLight]), JOYSTICK_PORT);
		joy.start();
		ds = new SPOTDriverStation(joy);
		log("Driver init success!");

		log("Initializing controller...");
		op = new CreateTeleopController(base, ds);
		op.setPrinting(true);
		log("Starting controller...");
		op.start();

		while (true) {
			Util.sleep(100000);
		}

	}

	public void sensorStateChanged(SensorEvent e, String key) {
		log("" + e.getData(key));

	}
}
